#include "Drive.h"

Drive::Drive(UINT32 leftJoystickChannel, UINT32 rightJoystickChannel, UINT32 leftMotorChannel, UINT32 rightMotorChannel)
{
	leftMotor = new Victor(leftMotorChannel);
	rightMotor = new Victor(rightMotorChannel);
	stick1 = new Joystick(leftJoystickChannel);
	stick2 = new Joystick(rightJoystickChannel);
}

void Drive::HandleDrive()
{
	float leftVal = Limit(-(stick1->GetY()));
	float rightVal = Limit(-(stick2->GetY()));
	if(stick1->GetTrigger() && stick2->GetTrigger())
	{
		leftVal = (leftVal * leftVal * leftVal);
		rightVal = (rightVal * rightVal * rightVal);
		float avgVal = (leftVal + rightVal) / 2;
		rightVal = avgVal;
		leftVal = avgVal;
	}
	else if(stick1->GetTop() && stick2->GetTop())
	{
		leftVal = 0.0;
		rightVal = 0.0;
	}
	else
	{
		leftVal = (leftVal * leftVal * leftVal);
		rightVal = (rightVal * rightVal * rightVal);
	}
	this->SetMotors(leftVal,(-1.0 * rightVal));
}

void Drive::SetDriveSpeed(float leftSpeed, float rightSpeed)
{
	this->SetMotors(leftSpeed, rightSpeed);
}

float Drive::GetLeftMotorSpeed() const
{
	return leftMotor->Get();
}

float Drive::GetRightMotorSpeed() const
{
	return rightMotor->Get();
}

void Drive::SetMotors(float leftVal, float rightVal)
{
	leftMotor->Set(leftVal);
	rightMotor->Set(rightVal);
}

float Drive::Limit(float num)
{
	if (num > 1.0)
	{
		return 1.0;
	}
	if (num < -1.0)
	{
		return -1.0;
	}
	return num;
}

Drive::~Drive()
{
	delete leftMotor;
	delete rightMotor;
	delete stick1;
	delete stick2;
}
